#ifndef P2ELEMENT_H_
#define P2ELEMENT_H_

#include "Core.h"
#include "Aabb2.h"
#include "Transform2.h"
#include "Element.h"

#define MAX_ANGULAR_VELOCITY	2.f
#define MAX_LINEAR_VELOCITY		1000.f

#define ENTITY_INITIALISED		(1<<0)

namespace p2 {

class World;
class Entity: public RealTimeType
{
DECLARE_RTT(Entity, RealTimeType);

public:
	typedef std::set<Entity* , std::less<Entity* >, boost::fast_pool_allocator<Entity* > > Set;

public:
	inline Entity(void* userData);
	inline Entity(Vector2<real> position, real rotation, void* userData);
	virtual ~Entity();

	static Entity* create(Vector2<real> position, real rotation, void* userData);
	static Entity* create(void* userData);

	bool initialise(real64 dT);
	void updateMotionBounds(real64 dT);
	void updateWorldSpace();
	bool contains(Vector2<real> point, Element*& element);
	void updateBroadphaseObjects(real64 dT);
	/*
	 * element management
	 */
	bool attach(Element* element);
	bool detach(Element* element);
	/*
	 * integration
	 */
	void integrateVelocity(real64 dT);
	void integratePosition(real64 dT);

    inline real getK(Vector2<real> r, Vector2<real> n) const;
	static inline real K(Entity const* a, Entity const* b, Vector2<real> ra, Vector2<real> rb, Vector2<real> n);
    static inline real invK(Entity const* a, Entity const* b, Vector2<real> ra, Vector2<real> rb, Vector2<real> n);

	inline void updateKineticEnergy();

	inline Vector2<real> toLocalSpace(Vector2<real> worldPoint);

	Transform2<real> getFutureWorldTransform(real64 dT);

	inline void addPointForce(Vector2<real> const& force,
		const Vector2<real>& point);
	inline void addCenterForce(Vector2<real> const& force);
	inline void addTorque(real torque);
	inline void addImpulse(Vector2<real> const& i, Vector2<real> const& r);
	inline void addLinearImpulse(Vector2<real> const& impulse);
	inline void addAngularImpulse(real impulse);
	inline void clearForces();
	/*
	 * access
	 */
	inline real getA() const;
	inline void setA(real a);
	inline Aabb2<real> const& getAabb() const;
	inline Aabb2<real>& getAabb();
	inline void setAabb(const Aabb2<real>& aabb);
	inline const Element::Set& getElements() const;
	inline void setElements(const Element::Set& elements);
	inline uint32 getFlags() const;
	inline void setFlags(uint32 flags);
	inline const Vector2<real>& getForce() const;
	inline void setForce(const Vector2<real>& force);
	inline const Vector2<real>& getHalfExtent() const;
    inline Vector2<real>& getHalfExtent();
	inline void setHalfExtent(const Vector2<real>& halfExtent);
	inline const String& getId() const;
	inline void setId(const String& id);
	inline real getInertia() const;
	inline void setInertia(real inertia);
	inline real getInvInertia() const;
	inline void setInvInertia(real invInertia);
	inline real getInvMass() const;
	inline void setInvMass(real invMass);
	inline real getKe() const;
	inline void setKe(real ke);
	inline real getMass() const;
	inline void setMass(real mass);
	inline const World* getOwner() const;
	inline void setOwner(World* world);
	inline const Vector2<real>& getScale() const;
	inline void setScale(const Vector2<real>& scale);
	inline real getTorque() const;
	inline void setTorque(real torque);
	inline const Transform2<real>& getTransform() const;
	inline void setTransform(const Transform2<real>& transform);
	inline void* getUserData() const;
	inline void setUserData(void* userData);
	inline const Vector2<real>& getV() const;
	inline void setV(const Vector2<real>& v);
	inline void incV(const Vector2<real>& v);
	inline real getW() const;
	inline void setW(real w);
	inline void incW(real dw);
	Vector2<real> const& getCenterOfMass() const
	{
		return _centerOfMass;
	}
	Vector2<real> const& getLocalOrigin() const
	{
		return _localOrigin;
	}
	Vector2<real> const& getX() const
	{
		return _x;
	}
	void setX(Vector2<real> const& x)
	{
		_x = x;
	}

protected:
	virtual void updateTransform();

protected:
	uint32 _flags;
	
    String _id;
	World* _owner;
	/*
	 * static properties
	 */
	real _mass;
	real _invMass;
	real _inertia;
	real _invInertia;
	/*
	 * kinetic/spatial properties
	 */
	Vector2<real> _x, _v;
	real _a, _w;
	Vector2<real> _scale;
	Vector2<real> _force;
	real _torque;
	real _Ke;

	Transform2<real> _transform;

	Vector2<real> _centerOfMass;
	Vector2<real> _localOrigin;

    Vector2<real> _halfExtent;
	Aabb2<real> _aabb;

	Element::Set _elements;

	void* _userData;
};

inline Entity::Entity(void* userData)
:   _flags(0), _owner(NULL), _mass(0.f), _invMass(0.f), _inertia(0.f), _invInertia(0.f),
	_x(Vector2<real>::ZERO), _v(Vector2<real>::ZERO), _a(0.f), _w(0.f),
	_scale(Vector2<real>::ONE),
	_force(Vector2<real>::ZERO), _torque(0.f), _Ke(0.f),
	_transform(),
	_centerOfMass(Vector2<real>::ZERO), _localOrigin(Vector2<real>::ZERO),
	_halfExtent(Vector2<real>::ZERO), _aabb(),
	_elements(),
	_userData(userData)
{
}

inline Entity::Entity(Vector2<real> x, real a, void* userData)
:   _flags(0), _owner(NULL), _mass(0.f), _invMass(0.f),_inertia(0.f), _invInertia(0.f),
	_x(x), _v(Vector2<real>::ZERO), _a(0.f), _w(a),
	_scale(Vector2<real>::ONE),
	_force(Vector2<real>::ZERO), _torque(0.f), _Ke(0.f),
	_transform(),
	_centerOfMass(Vector2<real>::ZERO), _localOrigin(Vector2<real>::ZERO),
	_halfExtent(Vector2<real>::ONE), _aabb(),
	_elements(),
	_userData(userData)
{
}

/*
 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
 * 'The following matrix has the property that the result of multiplying it
 * to an impulse is the velocity change this impulse causes:'
 */
inline real Entity::getK(Vector2<real> r, Vector2<real> n) const
{
    real cr = r.perp().dot(n);
    return (getInvMass() + cr * getInvInertia() * cr);
}

/*
 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
 * 'The following matrix has the property that the result of multiplying it
 * to an impulse is the velocity change this impulse causes:'
 */
inline real Entity::K(Entity const* a, Entity const* b, Vector2<real> ra, Vector2<real> rb, Vector2<real> n)
{
    return (a ? a->getK(ra, n) : 0.f) + (b ? b->getK(rb, n) : 0.f);
}

/*
 * REF: http://i31www.ira.uka.de/docs/Paper_VC05.pdf
 * 'The following matrix has the property that the result of multiplying it
 * to an impulse is the velocity change this impulse causes:'
 */
inline real Entity::invK(Entity const* a, Entity const* b, Vector2<real> ra, Vector2<real> rb, Vector2<real> n)
{
	real K = Entity::K(a, b, ra, rb, n);
    return (K != 0.0f) ? 1.0f / K : 0.0f;
}

inline real Entity::getMass() const
{
    return _mass;
}

inline void Entity::setMass(real mass)
{
    _mass = Math<real>::Min(mass, INFINITE_MASS);
    _invMass = _mass == INFINITE_MASS ? 0.f : (1.f / _mass);
}

inline real Entity::getInvMass() const
{
    return _invMass;
}

inline Aabb2<real> const& Entity::getAabb() const
{
    return _aabb;
}

inline Aabb2<real>& Entity::getAabb()
{
    return _aabb;
}

inline real Entity::getInertia() const
{
    return _inertia;
}

inline void Entity::setInertia(real inertia)
{
    _inertia = inertia;
    _invInertia = (_inertia > 0.f) ? (1.f / _inertia) : 0.f;
}

inline real Entity::getInvInertia() const
{
    return _invInertia;
}

inline void Entity::setInvInertia(real invInertia)
{
    _invInertia = invInertia;
    _inertia = (_invInertia > 0.f) ? (1.f / _invInertia) : 0.f;
}

inline void Entity::updateKineticEnergy()
{
	_Ke = 0.5f * (getMass() * Vector2<real>::squaredLength(_v) + getInertia() * (_w * _w));
}

inline Vector2<real> Entity::toLocalSpace(Vector2<real> worldPoint)
{
	return _transform.transformIntoLocalSpace(worldPoint) - _centerOfMass;
}

inline void Entity::addPointForce(Vector2<real> const& f, Vector2<real> const& r)
{
	_force += f;
	_torque += r.perp().dot(f);
}

inline void Entity::addCenterForce(Vector2<real> const& f)
{
	_force += f;
}

inline void Entity::addTorque(real t)
{
	_torque += t;
}

inline void Entity::addImpulse(Vector2<real> const& i, Vector2<real> const& r)
{
	_v += getInvMass() * i;
	_w += getInvInertia() * r.perp().dot(i);
}

inline void Entity::addLinearImpulse(Vector2<real> const& i)
{
	_v += getInvMass() * i;
}

inline void Entity::addAngularImpulse(real i)
{
	_w += getInvInertia() * i;
}

inline void Entity::clearForces()
{
	_force = Vector2<real>::ZERO;
	_torque = 0.f;
}

real Entity::getA() const
{
	return _a;
}

void Entity::setA(real a)
{
	_a = a;
}

void Entity::setAabb(const Aabb2<real>& aabb)
{
	_aabb = aabb;
}

const Element::Set& Entity::getElements() const
{
	return _elements;
}

void Entity::setElements(const Element::Set& elements)
{
	_elements = elements;
}

uint32 Entity::getFlags() const
{
	return _flags;
}

void Entity::setFlags(uint32 flags)
{
	_flags = flags;
}

const Vector2<real>& Entity::getForce() const
{
	return _force;
}

void Entity::setForce(const Vector2<real>& force)
{
	_force = force;
}

const Vector2<real>& Entity::getHalfExtent() const
{
	return _halfExtent;
}

void Entity::setHalfExtent(const Vector2<real>& halfExtent)
{
	_halfExtent = halfExtent;
}

const String& Entity::getId() const
{
	return _id;
}

void Entity::setId(const String& id)
{
	_id = id;
}

void Entity::setInvMass(real invMass)
{
	_invMass = invMass;
}

real Entity::getKe() const
{
	return _Ke;
}

void Entity::setKe(real ke)
{
	_Ke = ke;
}

const World* Entity::getOwner() const
{
	return _owner;
}

void Entity::setOwner(World* world)
{
	_owner = world;
}

const Vector2<real>& Entity::getScale() const
{
	return _scale;
}

void Entity::setScale(const Vector2<real>& scale)
{
	_scale = scale;
}

real Entity::getTorque() const
{
	return _torque;
}

void Entity::setTorque(real torque)
{
	_torque = torque;
}

const Transform2<real>& Entity::getTransform() const
{
	return _transform;
}

void Entity::setTransform(const Transform2<real>& transform)
{
	_transform = transform;
}

void* Entity::getUserData() const
{
	return _userData;
}

void Entity::setUserData(void* userData)
{
	_userData = userData;
}

const Vector2<real>& Entity::getV() const
{
	return _v;
}

void Entity::setV(const Vector2<real>& v)
{
	_v = v;
}

void Entity::incV(const Vector2<real>& v)
{
	_v += v;
}

real Entity::getW() const
{
	return _w;
}

void Entity::setW(real w)
{
	_w = w;
}

void Entity::incW(real dw)
{
	_w += dw;
}

}

#endif /* P2COMPONENT_H_ */
